#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import serial
import time
import scipy
import numpy
from DelphinROSv2.msg import tsl_setpoints
from DelphinROSv2.msg import tail_setpoints
from DelphinROSv2.msg import position
from DelphinROSv2.msg import compass
from DelphinROSv2.msg import depth_control
from std_msgs.msg import Float32
from std_msgs.msg import Bool
from scipy import stats

#### DEFINE GLOBAL VARIABLES ####
global flag
global cur_depthd
global cur_compass
global controller_onOff

################################################################################
#### CONTROLLER PARAMETERS #####################################################
################################################################################

def set_params():
    global DC
    
### General ###
    DC.CL             = 0.0114
    DC.buoyancy       = 7
    DC.min_int_error  =-5
    DC.max_int_error  = 5
    
### CS Pitch Controller ###
    DC.CS_pitch_Pgain =  4
    DC.CS_pitch_Igain =  0         #0.0001
    DC.CS_pitch_Pmin  = -15
    DC.CS_pitch_Pmax  =  5
    DC.CS_pitch_Imin  = -5
    DC.CS_pitch_Imax  =  5
    DC.CS_pitch_min   = -20
    DC.CS_pitch_max   =  5
    
### CS Controller ###
    DC.CS_Pgain       =  1.5
    DC.CS_Igain       =  0
    DC.CS_min         = -30 
    DC.CS_max         =  30
    
### Thrust Controller ###
    DC.Thrust_Pgain =  3
    DC.Thrust_Igain =  0.1
    DC.Thrust_Dgain =  3
    DC.Thrust_Pmin  = -2
    DC.Thrust_Pmax  =  10
    DC.Thrust_Imin  = -2.5
    DC.Thrust_Imax  =  2.5
    DC.Thrust_Dmin  = -5
    DC.Thrust_Dmax  =  5
    DC.Thrust_Smin  =  0
    DC.Thrust_Smax  =  2000    

################################################################################
#### CONTROL SURFACE CONTROLLER ################################################
################################################################################

def CS_controller(error, int_error, der_error, cur_compass):
    global DC

### Pitch Controller ###########################################################
    if speed > 0.4:
        DC.CS_ss_pitch_angle = -DC.buoyancy/(250*DC.CL*speed*abs(speed))
    else:
        DC.CS_ss_pitch_angle = 0;  
        
    DC.CS_pitch_Pterm      = limits((-error * DC.CS_pitch_Pgain), DC.CS_pitch_Pmin, DC.CS_pitch_Pmax)
    DC.CS_pitch_Iterm      = limits((-int_error * DC.CS_pitch_Igain), DC.CS_pitch_Imin, DC.CS_pitch_Imax)
    
    if speed > 0.4:
        DC.CS_pitch_demand = (DC.CS_ss_pitch_angle + DC.CS_pitch_Pterm + DC.CS_pitch_Iterm)/(speed*abs(speed))
    else:
        DC.CS_pitch_demand = (DC.CS_ss_pitch_angle + DC.CS_pitch_Pterm + DC.CS_pitch_Iterm)/(0.2); 
        
    DC.CS_pitch_demand     = limits(DC.CS_pitch_demand, DC.CS_pitch_min, DC.CS_pitch_max)
    
### CS Controller ##############################################################
    DC.pitch_error   = DC.CS_pitch_demand - cur_compass.pitch
    DC.CS_Pterm      = (DC.pitch_error * DC.CS_Pgain)
    if speed > 0.4:
        CS_demand    = limits((DC.CS_Pterm)/(speed*abs(speed)),DC.CS_min,DC.CS_max)
    else:
        CS_demand    = limits((DC.CS_Pterm)/(0.2),DC.CS_min,DC.CS_max)
    
    DC.CSp = CS_demand
    DC.CSs = CS_demand
        
### Estimated Z force ##########################################################
    DC.estLiftForce = 250*DC.CL*speed*abs(speed)*cur_compass.pitch

    return [DC.CSp, DC.CSs, DC.estLiftForce]
    
################################################################################
########## THRUST CONTROLLER ###################################################
################################################################################

def Thrust_controller(error, int_error, der_error, cur_compass, estLiftForce):

    DC.Thrust_Pterm = limits(error*DC.Thrust_Pgain, DC.Thrust_Pmin, DC.Thrust_Pmax)
    DC.Thrust_Iterm = limits(int_error*DC.Thrust_Igain, DC.Thrust_Imin, DC.Thrust_Imax)
    DC.Thrust_Dterm = limits(der_error*DC.Thrust_Dgain, DC.Thrust_Dmin, DC.Thrust_Dmax)
    
    if not numpy.isnan(DC.Thrust_Dterm):
        DC.Thrust_total = DC.Thrust_Pterm + DC.Thrust_Iterm + DC.Thrust_Dterm + (DC.buoyancy/2) - DC.estLiftForce
    else:
        DC.Thrust_total = DC.Thrust_Pterm + DC.Thrust_Iterm + (DC.buoyancy/2) - DC.estLiftForce
        
    DC.thruster0 = int(limits((numpy.sign(DC.Thrust_total)*(60*(numpy.abs(0.91*DC.Thrust_total)/(1000*0.46*0.07**4))**0.5)), DC.Thrust_Smin, DC.Thrust_Smax))
    DC.thruster1 = int(limits((numpy.sign(DC.Thrust_total)*(60*(numpy.abs(1.11*DC.Thrust_total)/(1000*0.46*0.07**4))**0.5)), DC.Thrust_Smin, DC.Thrust_Smax))
    
    return [DC.thruster0, DC.thruster1]

################################################################################
########## MAIN CONTROL LOOP ###################################################
################################################################################

def main_control_loop():

    #### SETUP ####
        global flag
        global cur_depthd
        global cur_compass
        global controller_onOff
        global speed
        global DC
        
        DC = depth_control()
        flag             = False
        cur_depthd       = Float32()
        cur_compass      = compass()
        speed            = 0
        controller_onOff = Bool()
        set_params()
        
################################################################################        
################################################################################
        #### MAIN CONTROLLER LOOP ####
        while not rospy.is_shutdown():

            if flag == True and controller_onOff == True:
                    # Get system state #
                    [error, int_error, der_error] = system_state(cur_depthd, cur_compass, DC.min_int_error, DC.max_int_error)

                    # Control Surface Controller # Nb CSp = Sternplane port, CSt = Rudder top
                    [CSp, CSs, estLiftForce] = CS_controller(error, int_error, der_error, cur_compass)
                        
                    # Thruster controller # 
                    [thruster0, thruster1] = Thrust_controller(error, int_error, der_error, cur_compass, estLiftForce)

                    pub_tail.publish(cs0 = CSp, cs1 = CSs)
                    pub_tsl.publish(thruster0 = thruster0, thruster1 = thruster1)
                    pub_DC.publish(DC)
                    
                    flag = False
######## END OF LOW LEVEL CONTROLLER ###########################################
################################################################################
################################################################################

################################################################################
######## CALCULATE CURRENT SYSTEM STATES #######################################
def system_state(cur_depthd, cur_compass, min_int_error, max_int_error):
    global time_zero
    global int_error
    global der_array_size
    global time_array
    global sample
    global cumulative_time
    global DC
    
    try:
        dt = time.time() - time_zero				# Calculate dt time between last loop interation
        time_zero = time.time()
    except:
        time_zero = time.time()                                 # On first loop define time zero and int_error
        dt = time.time() - time_zero
        int_error = 0
        der_array_size = 100
        time_array = numpy.zeros(der_array_size)
        sample = numpy.zeros(der_array_size)
        cumulative_time = numpy.zeros(der_array_size)

### ERROR ###
    error = DC.error = (cur_depthd.data - cur_compass.depth)               # Calculate depth error
    
### INTEGRAL ###   
    int_error += dt*error					# Calculate the integral error       
    int_error = DC.int_error = limits(int_error,DC.min_int_error,DC.max_int_error)

### DERIVATIVE ###
    sample[1:der_array_size] = sample[0:(der_array_size-1)]	# Shift old values up in the array
    sample[0] = error				                # Set first array term to new depth value

    cumulative_time[1:der_array_size] = cumulative_time[0:(der_array_size-1)]	# Shift old values up in the array
    cumulative_time[0] = time_zero	
    
    coeffs = numpy.polyfit(cumulative_time, sample, 1)
    der_error = DC.der_error = coeffs[0]
    
    return [error, int_error, der_error]
################################################################################

################################################################################
def limits(value, min, max):       #Function to contrain within defined limits
    if value < min:				   
       value = min
    elif value > max:
       value = max
    return value

#### WHEN NEW MISSION OR COMPASS DATA ARRIVES UPDATE VALUES AND RESET FLAG ####
def depth_demand_cb(depthd):
    global flag
    global cur_depthd
    global DC
    cur_depthd = depthd
    DC.depth_demand = depthd.data
    flag = True   

def compass_cb(compass):
    global flag
    global cur_compass
    cur_compass = compass 
    DC.depth = compass.depth
    DC.pitch = compass.pitch  
    flag = True

def onOff_cb(onOff):
    global controller_onOff
    global flag
    controller_onOff=onOff.data
    
def speed_callback(data):
    global speed
    speed = data.forward_vel
    DC.speed = speed

#### INITIALISATION ####
if __name__ == '__main__':
    rospy.init_node('PID_depth_control')
   
    rospy.Subscriber('depth_demand', Float32, depth_demand_cb)
    rospy.Subscriber('compass_out', compass, compass_cb)
    rospy.Subscriber('position_dead', position, speed_callback)
    rospy.Subscriber('Depth_onOFF', Bool, onOff_cb)
    
    pub_tsl  = rospy.Publisher('TSL_setpoints_vertical', tsl_setpoints)
    pub_tail = rospy.Publisher('tail_setpoints_horizontal', tail_setpoints)
    pub_DC   = rospy.Publisher('Depth_controller_values', depth_control)
    
    rospy.loginfo("Depth controller online")

    main_control_loop()
